#include "CovarianceMatrix.h"

CovarianceMatrix::CovarianceMatrix(void)
{
}

CovarianceMatrix::~CovarianceMatrix(void)
{
}

CvMat CovarianceMatrix::computeCovarianceMatrix(IplImage* img)
{
	IplImage* img_gray = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U,1);
	cvCvtColor(img, img_gray, CV_BGR2GRAY);
	cvEqualizeHist(img_gray,img_gray);

	/* Calculate the derivatives of the grayscale image in the x and y directions 
	using a sobel operator and obtain 2 gradient images for the x and y directions */
	IplImage* drvX = cvCreateImage( cvGetSize( img ), IPL_DEPTH_16S, 1 );
	IplImage* drvY = cvCreateImage( cvGetSize( img ), IPL_DEPTH_16S, 1 );	
	IplImage* drvX32F = cvCreateImage( cvGetSize( img ), IPL_DEPTH_32F, 1 );
	IplImage* drvY32F = cvCreateImage( cvGetSize( img ), IPL_DEPTH_32F, 1 );

	IplImage* secDrvX = cvCreateImage( cvGetSize( img ), IPL_DEPTH_16S, 1 );
	IplImage* secDrvY = cvCreateImage( cvGetSize( img ), IPL_DEPTH_16S, 1 );	
	IplImage* secDrvX32F = cvCreateImage( cvGetSize( img ), IPL_DEPTH_32F, 1 );
	IplImage* secDrvY32F = cvCreateImage( cvGetSize( img ), IPL_DEPTH_32F, 1 );

	IplImage* drvXY = cvCreateImage( cvGetSize( img ), IPL_DEPTH_16S, 1 );
	IplImage* drvXY32F = cvCreateImage( cvGetSize( img ), IPL_DEPTH_32F, 1 );

	// dx, dy
	cvSobel( img_gray, drvX, 1, 0 );
	cvConvertScale( drvX, drvX32F );
	cvSobel( img_gray, drvY, 0, 1 );
	cvConvertScale( drvY, drvY32F );

	// dx2, dy2
	cvSobel( img_gray, secDrvX, 2, 0 );
	cvConvertScale( secDrvX, secDrvX32F );
	cvSobel( img_gray, secDrvY, 0, 2 );
	cvConvertScale( secDrvY, secDrvY32F );

	// dxy
	cvSobel( img_gray, drvXY, 1, 1 );
	cvConvertScale( secDrvX, drvXY32F );

	cvReleaseImage( &drvX );
	cvReleaseImage( &drvY );
	cvReleaseImage( &secDrvX );
	cvReleaseImage( &secDrvY );
	cvReleaseImage( &drvXY );
	cvReleaseImage(&img_gray);

	CvMat drvXMat;	
	CvMat drvYMat;	
	CvMat secDrvXMat;	
	CvMat secDrvYMat;	
	CvMat drvXYMat;	
	cvGetMat(drvX32F,&drvXMat);		
	cvGetMat(drvY32F,&drvYMat);
	cvGetMat(secDrvX32F,&secDrvXMat);
	cvGetMat(secDrvY32F,&secDrvYMat);
	cvGetMat(drvXY32F,&drvXYMat);

	int n = img->width * img->height; // number of pixels	
	CvMat drvXArr = cvMat(1,n,CV_32F,(float*)drvXMat.data.fl);
	CvMat drvYArr = cvMat(1,n,CV_32F,(float*)drvYMat.data.fl);
	CvMat secDrvXArr = cvMat(1,n,CV_32F,(float*)secDrvXMat.data.fl);
	CvMat secDrvYArr = cvMat(1,n,CV_32F,(float*)secDrvYMat.data.fl);
	CvMat drvXYArr = cvMat(1,n,CV_32F,(float*)drvXYMat.data.fl);	

	// z
	FILE *pFile;
	pFile = fopen("data.txt","w");
	void** data = new void*[n];
	int k = 0;
	for (int x = 0; x < drvXMat.rows; x++)
	{
		for (int y = 0; y < drvXMat.cols; y++)
		{
			// zk
			float* tempArr = new float(5);
			tempArr[0] = cvGetReal2D(&drvXMat,x,y);
			tempArr[1] = cvGetReal2D(&drvYMat,x,y);
			tempArr[2] = cvGetReal2D(&secDrvXMat,x,y);
			tempArr[3] = cvGetReal2D(&secDrvYMat,x,y);
			tempArr[4] = cvGetReal2D(&drvXYMat,x,y);
			CvMat* tMat = cvCreateMat(1,5,CV_32FC1);
			cvInitMatHeader(tMat,1,5,CV_32FC1,tempArr);
			data[k++] = tMat;
			fprintf(pFile,"%f %f %f %f %f\n",cvGetReal2D(&drvXMat,x,y),cvGetReal2D(&drvYMat,x,y),
				cvGetReal2D(&secDrvXMat,x,y),cvGetReal2D(&secDrvYMat,x,y),cvGetReal2D(&drvXYMat,x,y));
		}
	}
	fclose(pFile);

	float covMat[25];
	CvMat cvcovMat = cvMat( 5, 5, CV_32FC1, covMat );		
	cvCalcCovarMatrix( (const void **)&data[0], n, &cvcovMat, NULL, CV_COVAR_NORMAL);		
	cvConvertScale(&cvcovMat, &cvcovMat, 1.0/n - 1);		
	//cvNormalize(&cvcovMat, &cvcovMat, 1, 0, CV_C);
	return cvcovMat;
}

CvMat* CovarianceMatrix::computeEigenValues(CvMat c1, CvMat c2)
{
	CvMat* A = cvCreateMat(5,5,CV_32FC1);
	CvMat* eigenValues = cvCreateMat(5,1,CV_32FC1);
	cvInvert(&c1,&c1);
	cvMatMul(&c1,&c2,A);	
	//cvSVD(A,eigenValues);
	cvEigenVV(A,0,eigenValues);
	return eigenValues;
}
